//
// Created by geoffrey on 24-12-15.
//

#ifndef CONTROLLER_H
#define CONTROLLER_H

#include <iostream>
#include <iomanip>
#include <vector>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "Eular2Isometry.h"
#include <pangolin/pangolin.h>
#include "cross.h"
#include "MatlabEngine.hpp"
#include "MatlabDataArray.hpp"
#include "g.h"
using namespace Eigen;

class controller {
  private:
    vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>> State_d;
    Matrix<double, 6, 6>  control_parameters = MatrixXd::Zero(6, 6);
    Matrix<double, 6, 2>  state_d ;
    Matrix<double, 6, 2>  state_error ;
    Matrix<double, 3, 1>  bar_alpha = MatrixXd::Zero(3, 1);
    Matrix<double, 3, 1>  tau ;
    Matrix<double, 3, 1>  chi = MatrixXd::Zero(3, 1);
    Matrix<double, 3, 1>  bar_tau = MatrixXd::Zero(3, 1);

    double alpha_u_old =0 , alpha_q_old =0, alpha_r_old =0;

    double t = 0, h =0.001,
    T_theta = 0.1, T_psi = 0.1,T_alpha = 0.1,
    gamma_nu = 100, gamma_y = 50, gamma_z = 50, gamma_q = 0.1, gamma_r = 0.1,
    omega_theta = 0.15, omega_psi = 0.15,
    k_x = 10, k_theta = 10, k_psi = 15,
    lambda_alpha = 4, lambda_0 = 20,
    k_u = 8, k_q = 5, k_r = 5,
    k_chi1 = 1e7, k_chi2 = 1e7, k_chi3 = 1e7,
    k_alpha = 1,
    k_zeta1 = 10, k_zeta2 = 10, k_zeta3 = 10,
    lambda_chi1 = 0.01, lambda_chi2 = 0.01, lambda_chi3 = 0.01,
    tau_W_i_M = 10,
    tau_u_M = 200, tau_q_M = 50, tau_r_M = 50;

 public:

    controller(Matrix<double, 6, 2> state);
    void update(Matrix<double, 6, 2> state, Matrix<double, 6, 1> f);
    Matrix<double, 6, 1> getTau();
    vector<Matrix<double, 6, 2>,aligned_allocator<Matrix<double, 6, 2>>> getState_d();
};

Matrix<double, 6, 6> derivatives(double t, Matrix<double, 6, 6> _control_parameters, Matrix<double, 6, 1> f);

#endif //CONTROLLER_H
